import sys
import os

#sys.path.append("/home/laf/Desktop/Link to Final_Year_Project/dev/Python/socf")

from socf import conf
from socf.client import local
from socf.services.constants import *

def __convertServiceListToMap(list):
    map = {}
    for entry in list:
        map[entry[2]] = entry[1].__str__()
    return map

nav = None

def solve_maze():
    #creates configuration context
    configContext = conf.ConfigurationContext(os.path.join(os.getcwd(),  "conf"))
    #get service stub provider
    localProvider = local.LocalObjectStubProvider.createProvider(configContext, "dbus")
    #contact the discovery service
    discoveryStub = localProvider.getServiceClientStub(DISCOVERY_SERVICE_TYPE, "dbus:laf.socf/server/discoveryService")
    servicesMap = __convertServiceListToMap(discoveryStub.getCurrentServicesList())
    
    global nav
    nav = localProvider.getServiceClientStub(NAV_SERVICE_TYPE, servicesMap["Navigation_Simulated1"])
    sonar_front = localProvider.getServiceClientStub(SONAR_SERVICE_TYPE, servicesMap["Sonar_Simulated_Front1"])
    sonar_right = localProvider.getServiceClientStub(SONAR_SERVICE_TYPE, servicesMap["Sonar_Simulated_Right1"])

    nav.moveUp()
    print "running.."
    
    while 1:
        if not is_obstacle(sonar_right):
            nav.moveRight()
        elif is_obstacle(sonar_front) and is_obstacle(sonar_right):
            nav.moveLeft()
    
def is_obstacle(sonar):
    dist = sonar.getDistance()
    if dist < 3:
        return 1
    else:
        return 0

if __name__=="__main__":
    try:
        solve_maze()
    except KeyboardInterrupt:
        print "\nShutdown.."
        nav.stop()
    
